#include "efem_sf.h"

#include "iostream"

void efem_sf::linear::triangle(double x, double y, double x1, double x2, double x3, 
    double y1, double y2, double y3, unsigned int idx, valtype_e vt, 
    ordertype_e ot, double &xval, double &yval)
{
    // Assign output values to defaults
    xval = yval = 0.0;

    double len;
    if (idx == 0) len = sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1));
    else if (idx == 1) len = sqrt((x3-x2)*(x3-x2) + (y3-y2)*(y3-y2));
    else if (idx == 2) len = sqrt((x3-x1)*(x3-x1) + (y3-y1)*(y3-y1));
    else throw std::invalid_argument("efem_sf::linear::triangle(...) got an invalid calculating index.");

    nfem_sf::linear lsf;
    if (vt == Value)
    {
        xval = len*(lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,idx,nfem_sf::Value)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(idx+1)%3,nfem_sf::Dx)
                - lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(idx+1)%3,nfem_sf::Value)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,idx,nfem_sf::Dx));

        yval = len*(lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,idx,nfem_sf::Value)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(idx+1)%3,nfem_sf::Dy) 
                - lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(idx+1)%3,nfem_sf::Value)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,idx,nfem_sf::Dy));
    }
    else
    {
        xval = len*(lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,idx,nfem_sf::Dy)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(idx+1)%3,nfem_sf::Dx)
                - lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(idx+1)%3,nfem_sf::Dy)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,idx,nfem_sf::Dx)); // dVx/dy

        yval = len*(lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,idx,nfem_sf::Dx)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(idx+1)%3,nfem_sf::Dy) 
                - lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(idx+1)%3,nfem_sf::Dx)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,idx,nfem_sf::Dy)); // dVy/dx

        if (vt == Curl)
        {
            xval = yval = yval - xval;
        }
    }

    if (ot) // Ordered in reversed direction
    {
        xval *= -1.0;
        yval *= -1.0;
    }
    return;
}

void efem_sf::linear::tetrahedron(double x, double y, double z, double x1, double x2, double x3, double x4, 
    double y1, double y2, double y3, double y4, double z1, double z2, double z3, double z4, 
    unsigned int idx, valtype_e vt, ordertype_e ot, double &xval, double &yval, double &zval, 
    double &xval2, double &yval2, double &zval2)
{
    // Assign output values to defaults
    xval = yval = zval = 0.0;
    xval2= yval2= zval2= 0.0;

    double x12 = x2-x1, y12 = y2-y1, z12 = z2-z1;
	double x13 = x3-x1, y13 = y3-y1, z13 = z3-z1;
	double x14 = x4-x1, y14 = y4-y1, z14 = z4-z1;
	double crx = y12*z13-z12*y13;
	double cry = z12*x13-x12*z13;
	double crz = x12*y13-y12*x13;
	double scl = (x14*crx+y14*cry+z14*crz);
	// No need to get the absolute value here
	//if (scl < 0) scl = -1.0*scl;

    unsigned int l_id, k_id;
    double len;
    if (idx == 0)
    {
        l_id = 0; k_id = 1;
        len = sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1) + (z2-z1)*(z2-z1));
    }
    else if (idx == 1)
    {
        l_id = 0; k_id = 2;
        len = sqrt((x3-x1)*(x3-x1) + (y3-y1)*(y3-y1) + (z3-z1)*(z3-z1));
    }
    else if (idx == 2)
    {
        l_id = 0; k_id = 3;
        len = sqrt((x4-x1)*(x4-x1) + (y4-y1)*(y4-y1) + (z4-z1)*(z4-z1));
    }
    else if (idx == 3)
    {
        l_id = 1; k_id = 2;
        len = sqrt((x3-x2)*(x3-x2) + (y3-y2)*(y3-y2) + (z3-z2)*(z3-z2));
    }
    else if (idx == 4)
    {
        l_id = 1; k_id = 3;
        len = sqrt((x4-x2)*(x4-x2) + (y4-y2)*(y4-y2) + (z4-z2)*(z4-z2));
    }
    else if (idx == 5)
    {
        l_id = 2; k_id = 3;
        len = sqrt((x4-x3)*(x4-x3) + (y4-y3)*(y4-y3) + (z4-z3)*(z4-z3));
    }
    else throw std::invalid_argument("efem_sf::linear::tetrahedron(...) got an invalid calculating index.");

    if (vt == Value)
    {
        nfem_sf::linear lsf;
        xval = (lsf.tetrahedron(x,y,z,x1,x2,x3,x4,y1,y2,y3,y4,z1,z2,z3,z4,l_id,nfem_sf::Value)*lsf.tetrahedron(x,y,z,x1,x2,x3,x4,y1,y2,y3,y4,z1,z2,z3,z4,k_id,nfem_sf::Dx) 
            - lsf.tetrahedron(x,y,z,x1,x2,x3,x4,y1,y2,y3,y4,z1,z2,z3,z4,k_id,nfem_sf::Value)*lsf.tetrahedron(x,y,z,x1,x2,x3,x4,y1,y2,y3,y4,z1,z2,z3,z4,l_id,nfem_sf::Dx))*len;

        yval = (lsf.tetrahedron(x,y,z,x1,x2,x3,x4,y1,y2,y3,y4,z1,z2,z3,z4,l_id,nfem_sf::Value)*lsf.tetrahedron(x,y,z,x1,x2,x3,x4,y1,y2,y3,y4,z1,z2,z3,z4,k_id,nfem_sf::Dy) 
            - lsf.tetrahedron(x,y,z,x1,x2,x3,x4,y1,y2,y3,y4,z1,z2,z3,z4,k_id,nfem_sf::Value)*lsf.tetrahedron(x,y,z,x1,x2,x3,x4,y1,y2,y3,y4,z1,z2,z3,z4,l_id,nfem_sf::Dy))*len;

        zval = (lsf.tetrahedron(x,y,z,x1,x2,x3,x4,y1,y2,y3,y4,z1,z2,z3,z4,l_id,nfem_sf::Value)*lsf.tetrahedron(x,y,z,x1,x2,x3,x4,y1,y2,y3,y4,z1,z2,z3,z4,k_id,nfem_sf::Dz) 
            - lsf.tetrahedron(x,y,z,x1,x2,x3,x4,y1,y2,y3,y4,z1,z2,z3,z4,k_id,nfem_sf::Value)*lsf.tetrahedron(x,y,z,x1,x2,x3,x4,y1,y2,y3,y4,z1,z2,z3,z4,l_id,nfem_sf::Dz))*len;

        if (ot)  // Ordered in reversed direction
        {
            xval *= -1.0; // Vx
            yval *= -1.0; // Vy
            zval *= -1.0; // Vz
        }
        return;
    }

    // Gradient or Curl
    double a1, a2, b1, b2, c1, c2;
    if (idx == 0)
    {
        // edge 0->1
        a1 = -1.0*jacoby3(1.0, y2, z2, 1.0, y3, z3, 1.0, y4, z4)/scl;
		b1 = jacoby3(1.0, x2, z2, 1.0, x3, z3, 1.0, x4, z4)/scl;
		c1 = -1.0*jacoby3(1.0, x2, y2, 1.0, x3, y3, 1.0, x4, y4)/scl;

        a2 = jacoby3(1.0, y1, z1, 1.0, y3, z3, 1.0, y4, z4)/scl;
		b2 = -1.0*jacoby3(1.0, x1, z1, 1.0, x3, z3, 1.0, x4, z4)/scl;
		c2 = jacoby3(1.0, x1, y1, 1.0, x3, y3, 1.0, x4, y4)/scl;
    }
    else if (idx == 1)
    {
        // edge 0->2
        a1 = -1.0*jacoby3(1.0, y2, z2, 1.0, y3, z3, 1.0, y4, z4)/scl;
		b1 = jacoby3(1.0, x2, z2, 1.0, x3, z3, 1.0, x4, z4)/scl;
		c1 = -1.0*jacoby3(1.0, x2, y2, 1.0, x3, y3, 1.0, x4, y4)/scl;

        a2 = -1.0*jacoby3(1.0, y1, z1, 1.0, y2, z2, 1.0, y4, z4)/scl;
		b2 = jacoby3(1.0, x1, z1, 1.0, x2, z2, 1.0, x4, z4)/scl;
		c2 = -1.0*jacoby3(1.0, x1, y1, 1.0, x2, y2, 1.0, x4, y4)/scl;
    }
    else if (idx == 2)
    {
        // edge 0->3
        a1 = -1.0*jacoby3(1.0, y2, z2, 1.0, y3, z3, 1.0, y4, z4)/scl;
		b1 = jacoby3(1.0, x2, z2, 1.0, x3, z3, 1.0, x4, z4)/scl;
		c1 = -1.0*jacoby3(1.0, x2, y2, 1.0, x3, y3, 1.0, x4, y4)/scl;

        a2 = jacoby3(1.0, y1, z1, 1.0, y2, z2, 1.0, y3, z3)/scl;
		b2 = -1.0*jacoby3(1.0, x1, z1, 1.0, x2, z2, 1.0, x3, z3)/scl;
		c2 = jacoby3(1.0, x1, y1, 1.0, x2, y2, 1.0, x3, y3)/scl;
    }
    else if (idx == 3)
    {
        // edge 1->2
        a1 = jacoby3(1.0, y1, z1, 1.0, y3, z3, 1.0, y4, z4)/scl;
		b1 = -1.0*jacoby3(1.0, x1, z1, 1.0, x3, z3, 1.0, x4, z4)/scl;
		c1 = jacoby3(1.0, x1, y1, 1.0, x3, y3, 1.0, x4, y4)/scl;

        a2 = -1.0*jacoby3(1.0, y1, z1, 1.0, y2, z2, 1.0, y4, z4)/scl;
		b2 = jacoby3(1.0, x1, z1, 1.0, x2, z2, 1.0, x4, z4)/scl;
		c2 = -1.0*jacoby3(1.0, x1, y1, 1.0, x2, y2, 1.0, x4, y4)/scl;
    }
    else if (idx == 4)
    {
        // edge 1->3
        a1 = jacoby3(1.0, y1, z1, 1.0, y3, z3, 1.0, y4, z4)/scl;
        b1 = -1.0*jacoby3(1.0, x1, z1, 1.0, x3, z3, 1.0, x4, z4)/scl;
        c1 = jacoby3(1.0, x1, y1, 1.0, x3, y3, 1.0, x4, y4)/scl;

        a2 = jacoby3(1.0, y1, z1, 1.0, y2, z2, 1.0, y3, z3)/scl;
        b2 = -1.0*jacoby3(1.0, x1, z1, 1.0, x2, z2, 1.0, x3, z3)/scl;
        c2 = jacoby3(1.0, x1, y1, 1.0, x2, y2, 1.0, x3, y3)/scl;
    }
    else if (idx == 5)
    {
        // edge 2->3
        a1 = -1.0*jacoby3(1.0, y1, z1, 1.0, y2, z2, 1.0, y4, z4)/scl;
		b1 = jacoby3(1.0, x1, z1, 1.0, x2, z2, 1.0, x4, z4)/scl;
		c1 = -1.0*jacoby3(1.0, x1, y1, 1.0, x2, y2, 1.0, x4, y4)/scl;

        a2 = jacoby3(1.0, y1, z1, 1.0, y2, z2, 1.0, y3, z3)/scl;
		b2 = -1.0*jacoby3(1.0, x1, z1, 1.0, x2, z2, 1.0, x3, z3)/scl;
		c2 = jacoby3(1.0, x1, y1, 1.0, x2, y2, 1.0, x3, y3)/scl;
    }
    else throw std::invalid_argument("efem_sf::linear::tetrahedron(...) got an invalid calculating index.");

    if (vt == Gradient)
    {
        xval = (b1*a2 - b2*a1)*len; // dVx/dy
        xval2= (c1*a2 - c2*a1)*len; // dVx/dz
        yval = (a1*b2 - a2*b1)*len; // dVy/dx
        yval2= (c1*b2 - c2*b1)*len; // dVy/dz
        zval = (a1*c2 - a2*c1)*len; // dVz/dx
        zval2= (b1*c2 - b2*c1)*len; // dVz/dy
    }
    else
    {
        xval = 2.0*(b1*c2 - b2*c1)*len; // curl(V)_x
        yval = 2.0*(c1*a2 - c2*a1)*len; // curl(V)_y
        zval = 2.0*(a1*b2 - a2*b1)*len; // curl(V)_z
    }

    if (ot)  // Ordered in reversed direction
    {
        xval *= -1.0;
        yval *= -1.0;
        zval *= -1.0;
        xval2*= -1.0;
        yval2*= -1.0;
        zval2*= -1.0;
    }
    return;
}

void efem_sf::quadratic::triangle(double x, double y, double x1, double x2, double x3, 
    double y1, double y2, double y3, unsigned int idx, valtype_e vt, 
    ordertype_e ot, double &xval, double &yval)
{
    // Assign output values to defaults
    xval = yval = 0.0;

    double len;
    if (idx <= 1) len = sqrt((x2-x1)*(x2-x1) + (y2-y1)*(y2-y1));
    else if (idx <= 3) len = sqrt((x3-x2)*(x3-x2) + (y3-y2)*(y3-y2));
    else if (idx <= 5) len = sqrt((x3-x1)*(x3-x1) + (y3-y1)*(y3-y1));
    else if (idx == 6) len = sqrt((x3-x2)*(x3-x2) + (y3-y2)*(y3-y2));
    else if (idx == 7) len = sqrt((x3-x1)*(x3-x1) + (y3-y1)*(y3-y1));
    else throw std::invalid_argument("efem_sf::quadratic::triangle(...) got an invalid calculating index.");

    unsigned int n_idx, e_idx;
    double A, B;
    nfem_sf::linear lsf;
    if (vt == Value)
    {
        if (idx <= 5)
        {
            e_idx = idx/2;
            n_idx = (e_idx + idx%2)%3;

            A = 3.0*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,n_idx,nfem_sf::Value) - 1.0;

            B = lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,e_idx,nfem_sf::Value)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(e_idx+1)%3,nfem_sf::Dx)
                - lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(e_idx+1)%3,nfem_sf::Value)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,e_idx,nfem_sf::Dx);
            xval = len*A*B;

            B = lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,e_idx,nfem_sf::Value)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(e_idx+1)%3,nfem_sf::Dy)
                - lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(e_idx+1)%3,nfem_sf::Value)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,e_idx,nfem_sf::Dy);
            yval = len*A*B;
        }
        else
        {
            n_idx = idx%2;
            e_idx = n_idx + 1;

            A = 4.5*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,n_idx,nfem_sf::Value);

            B = lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,e_idx,nfem_sf::Value)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(e_idx+1)%3,nfem_sf::Dx)
                - lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(e_idx+1)%3,nfem_sf::Value)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,e_idx,nfem_sf::Dx);
            xval = len*A*B;

            B = lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,e_idx,nfem_sf::Value)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(e_idx+1)%3,nfem_sf::Dy)
                - lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(e_idx+1)%3,nfem_sf::Value)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,e_idx,nfem_sf::Dy);
            yval = len*A*B;
        }
    }
    else
    {
        double Ad, Bd;
        if (idx <= 5)
        {
            e_idx = idx/2;
            n_idx = (e_idx + idx%2)%3;

            A = 3.0*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,n_idx,nfem_sf::Value) - 1.0;
            Ad= 3.0*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,n_idx,nfem_sf::Dy);

            B = lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,e_idx,nfem_sf::Value)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(e_idx+1)%3,nfem_sf::Dx)
                - lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(e_idx+1)%3,nfem_sf::Value)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,e_idx,nfem_sf::Dx);
            Bd= lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,e_idx,nfem_sf::Dy)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(e_idx+1)%3,nfem_sf::Dx)
                - lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(e_idx+1)%3,nfem_sf::Dy)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,e_idx,nfem_sf::Dx);

            xval = len*(Ad*B + A*Bd); // dVx/dy

            Ad= 3.0*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,n_idx,nfem_sf::Dx);

            B = lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,e_idx,nfem_sf::Value)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(e_idx+1)%3,nfem_sf::Dy)
                - lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(e_idx+1)%3,nfem_sf::Value)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,e_idx,nfem_sf::Dy);
            Bd = lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,e_idx,nfem_sf::Dx)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(e_idx+1)%3,nfem_sf::Dy)
                - lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(e_idx+1)%3,nfem_sf::Dx)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,e_idx,nfem_sf::Dy);

            yval = len*(Ad*B + A*Bd); // dVy/dx
        }
        else
        {
            n_idx = idx%2;
            e_idx = n_idx + 1;

            A = 4.5*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,n_idx,nfem_sf::Value);
            Ad= 4.5*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,n_idx,nfem_sf::Dy);

            B = lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,e_idx,nfem_sf::Value)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(e_idx+1)%3,nfem_sf::Dx)
                - lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(e_idx+1)%3,nfem_sf::Value)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,e_idx,nfem_sf::Dx);
            Bd= lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,e_idx,nfem_sf::Dy)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(e_idx+1)%3,nfem_sf::Dx)
                - lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(e_idx+1)%3,nfem_sf::Dy)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,e_idx,nfem_sf::Dx);

            xval = len*(Ad*B + A*Bd); // dVx/dy

            Ad= 4.5*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,n_idx,nfem_sf::Dx);

            B = lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,e_idx,nfem_sf::Value)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(e_idx+1)%3,nfem_sf::Dy)
                - lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(e_idx+1)%3,nfem_sf::Value)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,e_idx,nfem_sf::Dy);
            Bd= lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,e_idx,nfem_sf::Dx)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(e_idx+1)%3,nfem_sf::Dy)
                - lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,(e_idx+1)%3,nfem_sf::Dx)*lsf.triangle(x,y,x1,x2,x3,y1,y2,y3,e_idx,nfem_sf::Dy);

            yval = len*(Ad*B + A*Bd); // dVy/dx
        }

        if (vt == Curl)
        {
            xval = yval = yval - xval;
        }
    }

    if (ot) // Ordered in reversed direction
    {
        xval *= -1.0;
        yval *= -1.0;
    }
    return;
}